/*
 * <nav_recip_common/robot.h>
 * nav_recip_common
 *
 *
 * Copyright (c) 2008-2012, University of North Carolina at Chapel Hill
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 *   * Redistributions of source code must retain the above copyright notice,
 *     this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above copyright notice,
 *     this list of conditions and the following disclaimer in the documentation
 *     and/or other materials provided with the distribution.
 *   * Neither the name of the University of North Carolina at Chapel Hill nor
 *     the names of its contributors may be used to endorse or promote products
 *     derived from this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 *
 *
 * Please send all bug reports to <geom@cs.unc.edu>.
 *
 * The authors may be contacted via:
 *
 *     Dept. of Computer Science
 *     201 S. Columbia St.
 *     Frederick P. Brooks, Jr. Computer Science Bldg.
 *     Chapel Hill, N.C. 27599-3175
 *     United States of America
 *
 *     http://gamma.cs.unc.edu/ROS/
 */

#ifndef NAV_RECIP_COMMON_ROBOT_H_
#define NAV_RECIP_COMMON_ROBOT_H_

#include <ostream>
#include <vector>
#include <ros/macros.h>
#include <nav_recip_common/vector2.h>

namespace nav_recip
{
class Robot;

/*!
 * \relates Robot
 *
 * A list of Robot \c const pointers.
 */
typedef std::vector<const Robot*> ConstRobotList;

/*!
 * \relates Robot
 *
 * A list of Robot pointers.
 */
typedef std::vector<Robot*> RobotList;

/*!
 * \relates Robot
 *
 * An iterator for a ConstRobotList.
 */
typedef std::vector<const Robot*>::iterator ConstRobotIterator;

/*!
 * \relates Robot
 *
 * An iterator for a RobotList.
 */
typedef std::vector<Robot*>::iterator RobotIterator;

/*!
 * \relates Robot
 *
 * A \c const iterator for a RobotList.
 */
typedef std::vector<Robot*>::const_iterator RobotConstIterator;

/*!
 * \headerfile <nav_recip/robot.h>
 *
 * A robot that avoids collisions with its neighbors.
 */
class ROS_HELPER_EXPORT Robot
{
public:
  /*!
   * Create a new Robot.
   */
  ROS_FORCE_INLINE ROS_HELPER_LOCAL Robot() : max_speed_(0.0f), radius_(0.0f), new_velocity_(), position_(),
      preferred_velocity_(), velocity_(), neighbors_() { }

  /*!
   * Create a new Robot with given bounding radius, current position, and
   * current linear velocity.
   */
  ROS_FORCE_INLINE ROS_HELPER_LOCAL Robot(float radius, const Vector2& position, const Vector2& velocity) :
      max_speed_(0.0f), radius_(radius), new_velocity_(), position_(position),  preferred_velocity_(),
      velocity_(velocity), neighbors_() { }

  /*!
   * Create a new Robot with given bounding radius, current position, current
   * linear velocity, preferred linear velocity, and maximum linear speed.
   */
  ROS_FORCE_INLINE ROS_HELPER_LOCAL Robot(float radius, const Vector2& position, const Vector2& velocity,
                                          const Vector2& preferred_velocity, float max_speed) : max_speed_(max_speed),
      radius_(radius), new_velocity_(), position_(position), preferred_velocity_(preferred_velocity),
      velocity_(velocity), neighbors_() { }

  /*!
   * Destroy this Robot.
   */
  virtual ~Robot() { }

  /*!
   * Return the maximum linear speed of this Robot.
   */
  ROS_FORCE_INLINE ROS_HELPER_LOCAL float getMaxSpeed() const { return max_speed_; }

  /*!
   * Return the list of neighbors of this Robot.
   */
  ROS_FORCE_INLINE ROS_HELPER_LOCAL const ConstRobotList& getNeighbors() const { return neighbors_; }

  /*!
   * Return the new linear velocity of this Robot.
   */
  ROS_FORCE_INLINE ROS_HELPER_LOCAL const Vector2& getNewVelocity() const { return new_velocity_; }

  /*!
   * Return the current position of this Robot.
   */
  ROS_FORCE_INLINE ROS_HELPER_LOCAL const Vector2& getPosition() const { return position_; }

  /*!
   * Return the preferred linear velocity of this Robot.
   */
  ROS_FORCE_INLINE ROS_HELPER_LOCAL const Vector2& getPreferredVelocity() const { return preferred_velocity_; }

  /*!
   * Return the bounding radius of this Robot.
   */
  ROS_FORCE_INLINE ROS_HELPER_LOCAL float getRadius() const { return radius_; }

  /*!
   * Return the current linear velocity of this Robot.
   */
  ROS_FORCE_INLINE ROS_HELPER_LOCAL const Vector2& getVelocity() const { return velocity_; }

  /*!
   * Set the maximum linear speed of this Robot.
   */
  ROS_FORCE_INLINE ROS_HELPER_LOCAL void setMaxSpeed(float max_speed) { max_speed_ = max_speed; }

  /*!
   * Set the list of neighbors of this Robot.
   */
  ROS_FORCE_INLINE ROS_HELPER_LOCAL void setNeighbors(const ConstRobotList& neighbors) { neighbors_ = neighbors; }

  /*!
   * Set the current position of this Robot.
   */
  ROS_FORCE_INLINE ROS_HELPER_LOCAL void setPosition(const Vector2& position) { position_ = position; }

  /*!
   * Set the preferred linear velocity of this Robot.
   */
  ROS_FORCE_INLINE ROS_HELPER_LOCAL void setPreferredVelocity(const Vector2& preferred_velocity) { preferred_velocity_ = preferred_velocity; }

  /*!
   * Set the bounding radius of this Robot.
   */
  ROS_FORCE_INLINE ROS_HELPER_LOCAL void setRadius(float radius) { radius_ = radius; }

  /*!
   * Set the current linear velocity of this Robot.
   */
  ROS_FORCE_INLINE ROS_HELPER_LOCAL void setVelocity(const Vector2& velocity) { velocity_ = velocity; }

  /*!
   * Compute the new linear velocity of this Robot that avoids collisions with
   * its neighbors.
   */
  virtual bool computeNewVelocity(float time_step) { return false; }

  float max_speed_;            // Maximum linear speed.
  float radius_;               // Bounding radius.
  Vector2 new_velocity_;       // New linear velocity to be set by computeNewVelocity().
  Vector2 position_;           // Current position.
  Vector2 preferred_velocity_; // Preferred linear velocity.
  Vector2 velocity_;           // Current linear velocity to be used by computeNewVelocity().
  ConstRobotList neighbors_;   // Neighbors with which to avoid collisions.
};
/*!
 * \relates Robot
 *
 * Insert a given Robot into a given output stream
 */
ROS_FORCE_INLINE ROS_HELPER_LOCAL std::ostream& operator<<(std::ostream& output_stream, const Robot& robot)
{
  output_stream << "[ Robot: radius = " << robot.getRadius() << ", position = " << robot.getPosition()
      << ", velocity = " << robot.getVelocity() << " ]";

  return output_stream;
}
}

#endif // NAV_RECIP_COMMON_ROBOT_H_
